#include "distance_map.h"
#include <stdio.h>

	bool operator<(const CellData& A, const CellData& B)
	{
		return A.mpMap->Cells[MAP_INDEX(A.mpMap, A.mnI, A.mnJ)].Dist > 
		A.mpMap->Cells[MAP_INDEX(B.mpMap, B.mnI, B.mnJ)].Dist;
	}

	CachedDistanceMap* GetDistanceMap(float Scale, float MaxDist)
	{
	    static CachedDistanceMap* Cdm = NULL;

		if(!Cdm || (Cdm->mdScale != Scale) || (Cdm->mdScale != MaxDist))
		{
			if(Cdm)
			    delete Cdm;
			Cdm = new CachedDistanceMap(Scale, MaxDist);
		}

	    return Cdm;
	}

	void Enqueue(DisMap* M, unsigned int I, unsigned int J, 
		     unsigned int SrcI, unsigned int SrcJ,
		     std::priority_queue<CellData>& Q,
		     CachedDistanceMap* Cdm,
		     unsigned char* Marked)
	{
		if(Marked[MAP_INDEX(M, I, J)])
			return;
		unsigned int Di = 0;

		if (I > SrcI){
			Di = I - SrcI;
		}
		else{
			Di = SrcI - I;
		}

		unsigned int Dj = 0;
		if (J > SrcJ){
			Dj = J - SrcJ;
		}
		else{
			Dj = SrcJ - J;
		}

		float Distance = Cdm->mpDistances[Di][Dj];

		if(Distance > Cdm->mnCellRadius)
			return;

		M->Cells[MAP_INDEX(M, I, J)].Dist = Distance * M->Scale;
		M->Cells[MAP_INDEX(M, I, J)].SrcI = SrcI;
		M->Cells[MAP_INDEX(M, I, J)].SrcJ = SrcJ;

		CellData Cell;
		Cell.mpMap = M;
		Cell.mnI = I;
		Cell.mnJ = J;
		Cell.mnSrcI = SrcI;
		Cell.mnSrcJ = SrcJ;

		Q.push(Cell);

		Marked[MAP_INDEX(M, I, J)] = 1;
	}

	void MapUpdateCspace(DisMap *M, float MaxDist)
	{
		unsigned char* Marked;
		std::priority_queue<CellData> Q;

		Marked = new unsigned char[M->SizeX*M->SizeY];
		memset(Marked, 0, sizeof(unsigned char) * M->SizeX*M->SizeY);

		M->MaxDist = MaxDist;

		CachedDistanceMap* Cdm = GetDistanceMap(M->Scale, M->MaxDist);

		CellData Cell;
		Cell.mpMap = M;
		for(int i=0; i<M->SizeX; i++)
		{
			Cell.mnSrcI = Cell.mnI = i;
			for(int j=0; j<M->SizeY; j++)
			{
				if(M->Cells[MAP_INDEX(M, i, j)].State == +1)
				{
					M->Cells[MAP_INDEX(M, i, j)].Dist = 0.0;
					M->Cells[MAP_INDEX(M, i, j)].SrcI = i;
					M->Cells[MAP_INDEX(M, i, j)].SrcJ = j;
					Cell.mnSrcJ = Cell.mnJ = j;
					Marked[MAP_INDEX(M, i, j)] = 1;
					Q.push(Cell);
				}
				else
				{
					M->Cells[MAP_INDEX(M, i, j)].Dist = MaxDist;
					M->Cells[MAP_INDEX(M, i, j)].SrcI = -1;
					M->Cells[MAP_INDEX(M, i, j)].SrcJ = -1;
				}
			}
		}

		while(!Q.empty())
		{
			CellData CurrentCell = Q.top();
			if(CurrentCell.mnI > 0)
			{
				Enqueue(M, CurrentCell.mnI-1, CurrentCell.mnJ, 
					CurrentCell.mnSrcI, CurrentCell.mnSrcJ,
				    Q, Cdm, Marked);
			}
			if(CurrentCell.mnJ > 0)
			{
				Enqueue(M, CurrentCell.mnI, CurrentCell.mnJ-1, 
					CurrentCell.mnSrcI, CurrentCell.mnSrcJ,
					Q, Cdm, Marked);
			}
			if((int)CurrentCell.mnI < M->SizeX - 1)
			{
				Enqueue(M, CurrentCell.mnI+1, CurrentCell.mnJ, 
					CurrentCell.mnSrcI, CurrentCell.mnSrcJ,
					Q, Cdm, Marked);
			}
			if((int)CurrentCell.mnJ < M->SizeY - 1)
			{
				Enqueue(M, CurrentCell.mnI, CurrentCell.mnJ+1, 
					CurrentCell.mnSrcI, CurrentCell.mnSrcJ,
					Q, Cdm, Marked);
			}

			Q.pop();
		}

		delete[] Marked;
	}
